/* $Header: sig_math.c,v 2.3 20/3./6. 4.:5.:0. dryden Exp $ */

/***********************************************************************/
/*                                                                     */
/*                  Copyright Hewlett-Packard Co. 1991                 */
/*                                                                     */
/***********************************************************************/

/***********************************************************************/
/* MODULE NAME: sig_math.c                                              */
/* ============                                                        */
/*                                                                     */
/* Author:        Lesley Freed                                         */
/*                                                                     */
/* MODULE PURPOSE: To provide math routines for use in the signal      */
/*                 path.                                               */
/*                                                                     */
/***********************************************************************/


/*****************************/
/*   GLOBAL INCLUDE FILES    */
/*****************************/

#include <typedefs.h>
#include <p_wkfile.h>
#include <wkfile.h>
#include <ramps.h>
#include <zone_config.h>
#include <zone_status.h>
#include <status.h>
#include <error.h>
#include <sig_types.h>
#include <sigpath.h>

/*****************************/
/*   LOCAL INCLUDE FILES     */
/*****************************/


/*****************************/
/*   GLOBAL VARIABLES        */
/*****************************/


/*****************************/
/*   EXTERNAL VARIABLES      */
/*****************************/


/*****************************/
/*   LOCAL DEFINES           */
/*****************************/


/*****************************/
/*   IMPLEMENT SECTION       */
/*****************************/

#ifndef mccabe
#pragma SECTION PROG=SysCode
#endif


/***********************************************************************/
/* FUNCTION: Scale3Hz                                                  */
/* ========                                                            */
/*                                                                     */
/* PURPOSE:  To calculate (16*Multiplicand + Remainder)/256.  This     */
/*    function does a 32*32-bit multiply to get a 64-bit result.       */
/*                                                                     */
/* RETURNED VALUE: Returned value is a data structure of type LDIV_TYPE*/
/*    which is 32:32 bits, which are the quotient and the remainder.   */
/*                                                                     */
/* GLOBALS USED: none                                                  */
/*                                                                     */
/* LIMITATIONS: Dependent on the operation of the 68332 C-Compiler     */
/*    from hp-lsd which puts the results of the function in d0:d1.     */
/*                                                                     */
/***********************************************************************/

LDIV_TYPE Scale3Hz( U_INT32 multiplicand,
                    U_INT32 remainder )
{

/* identical to return ( { multiplicand*16 + remainder }/256 ) */
/*  however, this uses a 64 bit intermediate                   */

/* according to the 68332 C Cross Compiler Reference Manual
   Section 3, page 3-18 (Register Usage)  D0,D1 are reserved
   for working registers, we can scribble them at will.  This
   section also indicates that D0 is used for a 32 bit return. */
printf("call Scale3Hz\n");
#ifndef mccabe
#pragma ASM

        move.l  (S_multiplicand,A6),d0
        mulu.l  #16,d1:d0               ; 16*signal -> d0:d1
        add.l   (S_remainder,A6),d0     ; add the remainder
        bcc.s   div2                    ; check for carry
        addq.l  #1,d1                   ; add the carry bit to the MS 32-bits
div2:
        divu.l  #256,d1:d0              ; (16*signal + rem)/256 -> 32r:32q

#pragma END_ASM
#endif

   (void)multiplicand;
   (void)remainder;
}


LDIV_TYPE ScaleAttn3Hz( U_INT32 multiplicand,
                    U_INT32 remainder )
{
printf("call ScaleAttn3Hz\n");
/* identical to return ( { multiplicand*32 + remainder }/64 )  */
/*  however, this uses a 64 bit intermediate                   */

/* according to the 68332 C Cross Compiler Reference Manual
   Section 3, page 3-18 (Register Usage)  D0,D1 are reserved
   for working registers, we can scribble them at will.  This
   section also indicates that D0 is used for a 32 bit return. */

/* Scale the attn output to match the 1V dac output by multiply- */
/* ing by 32/64 and then by 1.11 that compensates for the hw     */
/* gain on the dac.                                              */

#ifndef mccabe
#pragma ASM

        move.l  (S_multiplicand,A6),d0
        mulu.l  #71,d1:d0               ; 71*signal -> d0:d1
        add.l   (S_remainder,A6),d0     ; add the remainder
        bcc.s   div                     ; check for carry
        addq.l  #1,d1                   ; add the carry bit to the MS 32-bits
div:
        divu.l  #128,d1:d0              ; (71*signal + rem)/128 -> 32r:32q

#pragma END_ASM
#endif

   (void)multiplicand;
   (void)remainder;
}


/***********************************************************************/
/* FUNCTION: FiltSum                                                   */
/* ========                                                            */
/*                                                                     */
/* PURPOSE: To add a new data pt to the filter sum and subtract        */
/*          the oldest data point.                                     */
/*                                                                     */
/* RETURNED VALUE:  filt_sum                                           */
/*                                                                     */
/* LIMITATIONS:                                                        */
/*                                                                     */
/***********************************************************************/


BUNCH_SUM FiltSum( INT32 signal, INT32 old_signal, INT32 sum_hi, INT32 sum_lo)
{

printf("call FiltSum\n");
#ifndef mccabe
#pragma ASM

*  Subtract the oldest signal point from the new signal pt and add to the
*  running sum

        move.l  (S_signal,A6),d0        ; subtract old signal from

        sub.l   (S_old_signal,A6),d0    ;  new signal
        bvc.b   sub_ok                  ; check for overflow/underflow
        bmi.b   pos_delta               ; check for overflow
neg_delta:
        move.l  #0FFFFFFFFh,d1          ; negative delta
        bra.b   add_delta
pos_delta:
        moveq.l #0,d1                   ; positive delta
        bra.b   add_delta
sub_ok:
        bmi.b   neg_delta               ; check for overflow
        moveq.l #0,d1                   ; positive delta
add_delta:
        add.l   (S_sum_lo,A6),d0        ; add the LS 32-bits
        bcc     no_carry
        addq.l  #1,d1                   ; add the carry bit
no_carry:
        add.l   (S_sum_hi,A6),d1        ; add the MS 32-bits

#pragma END_ASM
#endif

   (void)signal;
   (void)old_signal;
   (void)sum_hi;
   (void)sum_lo;
}



/***********************************************************************/
/* FUNCTION: BunchSum32                                                */
/* ========                                                            */
/*                                                                     */
/* PURPOSE: To add a new data pt to the bunch sum and increment        */
/*          the bunch count, Bunch_Count. This basically adds a        */
/*          32-bit value to a 64-bit sum.                              */
/*                                                                     */
/* RETURNED VALUE:  bunch_sum                                          */
/*                                                                     */
/* LIMITATIONS:                                                        */
/*                                                                     */
/***********************************************************************/

BUNCH_SUM BunchSum32(INT32 signal, INT32 sum_hi, INT32 sum_lo)
{
printf("call BunchSum32\n");
#ifndef mccabe
#pragma ASM

; first convert the signal from 32-bit to 64-bit before adding

        move.l  (S_signal,A6),d0        ; d0 = LS 32-bits of signal
        bmi.b   neg_sig32
        moveq.l #0,d1                   ; positive signal
        bra.b   add_sig32
neg_sig32:
        move.l  #0FFFFFFFFh,d1          ; negative signal

; next add the new signal point

add_sig32:
        add.l   (S_sum_lo,A6),d0        ; add the LS 32-bits
        bcc.s   no_carry32              ; check for carry
        addq.l  #1,d1                   ; add the carry bit to the MS 32-bits
no_carry32:
        add.l   (S_sum_hi,A6),d1        ; add the MS 32-bits

#pragma END_ASM
#endif

   (void)signal;
   (void)sum_hi;
   (void)sum_lo;
}


/***********************************************************************/
/* FUNCTION: BunchSum                                                  */
/* ========                                                            */
/*                                                                     */
/* PURPOSE: To add a new data pt to the bunch sum and increment        */
/*          the bunch count, Bunch_Count. This basically adds a        */
/*          48-bit value to a 64-bit sum.                              */
/*                                                                     */
/* RETURNED VALUE:  bunch_sum                                          */
/*                                                                     */
/* LIMITATIONS:                                                        */
/*                                                                     */
/***********************************************************************/

BUNCH_SUM BunchSum(INT16 sig_hi, INT32 sig_lo, INT32 sum_hi, INT32 sum_lo)
{
printf("call BunchSum\n");
#ifndef mccabe
#pragma ASM

; first convert the signal from 48-bit to 64-bit before adding

        move.w  (S_sig_hi,A6),d1        ; d1 = MS 16-bits of signal (bits 47-32)
        bmi.b   neg_sig
        andi.l  #0000ffffh,d1           ; positive signal
        bra.b   add_offset
neg_sig:
        ori.l   #0FFFF0000h,d1          ; negative signal

add_offset:

        add.l   #8000h,d1               ; make the MS 32-bits of signal positive

; next add the new signal point

        move.l  (S_sig_lo,A6),d0        ; d0 = LS 32-bits of signal
        add.l   (S_sum_lo,A6),d0        ; add the LS 32-bits
        bcc.s   no_carry2               ; check for carry
        addq.l  #1,d1                   ; add the carry bit to the MS 32-bits
no_carry2:
        add.l   (S_sum_hi,A6),d1        ; add the MS 32-bits

#pragma END_ASM
#endif

   (void)sig_hi;
   (void)sig_lo;
   (void)sum_hi;
   (void)sum_lo;
}

/***********************************************************************/
/* FUNCTION: BoxCarSum3                                                */
/* ========                                                            */
/*                                                                     */
/* PURPOSE: To update a 64-bit 3-pt boxcar filter sum by adding in the */
/*          new data point and subtracting the oldest data point.      */
/*                                                                     */
/* RETURNED VALUE:  filt_bunch_sum                                     */
/*                                                                     */
/* LIMITATIONS:                                                        */
/*                                                                     */
/***********************************************************************/

void BoxCarSum3( pBUNCH_SUM psum, pBUNCH_SUM pfilt_sum, pBUNCH_SUM psum_table )
{
printf("call BoxCarSum3\n");
#ifndef mccabe
#pragma ASM

        move.l  a2,-(A7)                ; save registers

; Update the filter sum

        move.l  (S_psum,A6),a0          ; a0 = ptr to new bunch sum
        move.l  (S_pfilt_sum,A6),a1     ; a1 = ptr to sum of 3 bunch sums
        move.l  (S_psum_table,A6),a2    ; a2 = ptr to table of 3 bunch sums

; subtract the oldest data pt from the running sum

        move.l  (a1),d0                 ; d0 = LS 32-bits of sum of bunch sums
        move.l  (a1,4),d1               ; d1 = MS 32-bits of sum of bunch sums

        sub.l   (a2),d0                 ; subtract the LS 32-bits

        bcc.s   sub_ok1

        subq.l  #1,d1                   ; subtract the carry

sub_ok1:
        sub.l   (a2,4),d1               ; subtract the MS 32-bits

; add the new data pt to the running sum

        add.l   (a0),d0                 ; add the LS 32-bits

        bcc.s   add_ok1

        addq.l  #1,d1                   ; add the carry

add_ok1:
        add.l   (a0,4),d1               ; add the MS 32-bits

; update the filt sum

        move.l  d0,(a1)                 ; LS 32-bits of sum
        move.l  d1,(a1,4)               ; MS 32-bits of sum

; put the new data point in the table

        move.l  (a0),(a2)               ; LS 32-bits of sum
        move.l  (a0,4),(a2,4)           ; MS 32-bits of sum

        move.l  (a7)+,a2                ; restore registers

#pragma END_ASM
#endif

   (void)psum;
   (void)pfilt_sum;
   (void)psum_table;
}

/***********************************************************************/
/* FUNCTION: ResetBoxCarSum3                                           */
/* ========                                                            */
/*                                                                     */
/* PURPOSE: To initialize a 64-bit 3-pt boxcar filter sum to three     */
/*          times the bunch sum.                                       */
/*                                                                     */
/* RETURNED VALUE:  filt_bunch_sum in d0:d1                            */
/*                                                                     */
/* LIMITATIONS:                                                        */
/*                                                                     */
/***********************************************************************/

BUNCH_SUM ResetBoxCarSum3( pBUNCH_SUM psum )
{
printf("call ResetBoxCarSum3\n");
#ifndef mccabe
#pragma ASM

; Set up the registers

        move.l  (S_psum,A6),a0          ; a0 = ptr to the bunch sum

; Init the filt bunch sum

        move.l  (a0),d0                 ; d0 = low 32-bits of bunch sum
        move.l  (a0,4),d1               ; d1 = high 32-bits of bunch sum

; add the bunch sum to the running sum

        add.l   (a0),d0                 ; add the LS 32-bits

        bcc.s   add_ok2

        addq.l  #1,d1                   ; add the carry

add_ok2:
        add.l   (a0,4),d1               ; add the MS 32-bits

; add the bunch sum a second time to yield 3*bunch sum in d1:d0

        add.l   (a0),d0                 ; add the LS 32-bits

        bcc.s   add_ok3

        addq.l  #1,d1                   ; add the carry

add_ok3:
        add.l   (a0,4),d1               ; add the MS 32-bits

#pragma END_ASM
#endif

   (void)psum;
}

/***********************************************************************/
/* FUNCTION:  BunchOut                                                 */
/* ========                                                            */
/*                                                                     */
/* PURPOSE:  To calculate (Bunch Sum+ Remainder)/Bunch Count           */
/*                                                                     */
/* RETURNED VALUE:  bunch_out, a structure containing the calculated   */
/*                  data out and the 32-bit remainder                  */
/*                                                                     */
/* LIMITATIONS:                                                        */
/*                                                                     */
/***********************************************************************/

void BunchOut( INT32 divisor, INT32 offset, pBUNCH_SUM psum, pBUNCH_OUT pout )
{
printf("call BunchOut\n");
#ifndef mccabe
#pragma ASM

 xref _MULT_OUT_HI
 xref _MULT_OUT_LO
 xref _MS_DIV_REM
 xref _MS_DIV_QUO
 xref _LS_DIV_REM
 xref _LS_DIV_QUO

DEBUG equ 0
; Use as shown below:
; IFNE DEBUG
; ENDC


        move.l  d2,-(A7)                ; save registers

        move.l  (S_psum,A6),a0          ; a0 = ptr to bunch sum
        move.l  (S_pout,A6),a1          ; a1 = ptr to bunch out

        move.l  (a0),d0                 ; d0 = LS 32-bits of sum
        move.l  (a0,4),d1               ; d1 = MS 32-bits of sum

; ****************************************************************************
; multiply the sum of the last 3 bunch sums by 11
;  this will give us a gain of 33: 3X11
; ****************************************************************************

        muls.l  #11,d1                  ; 32X32 -> 32
        bvc.s   mult_ok1

        nop                             ; REVISIT

mult_ok1:

; multiply LS 32-bits by 11

        mulu.l  #11,d2:d0               ; 32X32 -> 64

; add MS 32-bits part of multiply to the MS 32-bits of the result

        add.l  d2,d1

; ****************************************************************************
; divide the result of the boxcar filter by Gain*( the # of readings bunched )
; NOTE:  Gain = 32 for signals from the detector DSP and
;        Gain = 33 for all non-DSP signals (giving a overall gain of 1)
; ****************************************************************************

; add in the remainder from the last divide

        add.l   (a1,6),d0               ; d0 = LS 32-bits of sum + rem
        bcc.s   rem_add_ok              ; check for carry
        addq.l  #1,d1                   ; add carry

rem_add_ok:

; divide MS 32-bits by divisor

        divul.l (S_divisor,a6),d2:d1    ; 32/32 -> 32r:32q

; remainder is now d2:d0
; now divide 64-bit remainder by divisor

; subtract out the offset added to ensure unsigned arithmatic
; offset is a parameter since gain is 33/32 OR 32/32

        sub.l   (S_offset,a6),d1

; check for overflow/underflow and clamp if needed

        cmpi.l   #7fffh,d1
        ble.s   div_ok
        move.w  #7fffh,d1               ; clamp at maximum
        bra.s   div_ok

        cmpi.l   #0ffff8000h,d1
        bge.s   div_ok
        move.w  #8000h,d1               ; clamp at minimum

; store result in MS 16-bits of bunch out

div_ok:
        move.w  d1,(a1)

        divu.l  (S_divisor,a6),d2:d0    ; 64/32 -> 32r:32q

; store the quotient in the LS 32-bits of bunch out

        move.l  d0,(a1,2)

; store the remainder in 32-bit bunch out remainder

        move.l  d2,(a1,6)

        move.l  (a7)+,d2                ; restore registers

#pragma END_ASM
#endif
   (void)divisor;
   (void)offset;
   (void)psum;
   (void)pout;
}


/***********************************************************************/
/* FUNCTION:  BunchOut32                                               */
/* ========                                                            */
/*                                                                     */
/* PURPOSE:  To calculate (Bunch Sum+ Remainder)/Bunch Count           */
/*                                                                     */
/* RETURNED VALUE:  bunch_out, a structure containing the calculated   */
/*                  data out and the 32-bit remainder                  */
/*                                                                     */
/* LIMITATIONS:                                                        */
/*                                                                     */
/***********************************************************************/

BUNCH_OUT32 BunchOut32(INT32 sum_hi,
                       INT32 sum_lo,
                       U_INT32 divisor,
                       INT32   remainder)
{

printf("call BunchOut32\n");
#ifndef mccabe
#pragma ASM
        move.l  d2,-(A7)                ; save registers

        move.l  (S_sum_hi,A6),d1        ; d1 = 32-MSbits of sum
        move.l  (S_sum_lo,A6),d0        ; d0 = 32-LSbits of sum
        move.l  (S_remainder,A6),d2     ; d2 = remainder from last divide
        bpl.s   pos_rem32               ; should we sign extend?
        add.l   #0ffffffffh,d1          ; negative remainder
pos_rem32
        add.l   d2,d0                   ; add the remainder
        bcc.s   do_div32                ; check for overflow
        addq.l  #1,d1                   ; add the carry bit to the MS 32-bits
do_div32:
        divs.l  (S_divisor,A6),d1:d0    ; (signal + rem)/div -> 32r:32q

        move.l  (a7)+,d2                ; restore registers
#pragma END_ASM
#endif
   (void)divisor;
   (void)remainder;
   (void)sum_hi;
   (void)sum_lo;
}

/***********************************************************************/
/* FUNCTION: TblMult                                                   */
/* ========                                                            */
/*                                                                     */
/* PURPOSE:  To calculate a 32-bit by 32-bit multiply followed by a    */
/*           shift right by 14.  This function does a 32*32-bit        */
/*           multiply to get a 64-bit result.                          */
/*                                                                     */
/* RETURNED VALUE: Returned value is a unsigned 32-bit value.          */
/*                                                                     */
/* GLOBALS USED: none                                                  */
/*                                                                     */
/* LIMITATIONS: does no bounds checking for overflow of the result     */
/*                                                                     */
/***********************************************************************/

U_INT32 TblMult( U_INT32 mult1, U_INT32 mult2 )
{
printf("call TblMult\n");

/* identical to return ( mult1*mult2 )>>14          */
/*  however, this uses a 64 bit intermediate        */

/* according to the 68332 C Cross Compiler Reference Manual
   Section 3, page 3-18 (Register Usage)  D0,D1 are reserved
   for working registers, we can scribble them at will.  This
   section also indicates that D0 is used for a 32 bit return. */

#ifndef mccabe
#pragma ASM

        move.l  d2,-(A7)                ; save registers

        move.l  (S_mult1,A6),d0
        mulu.l  (S_mult2,A6),d1:d0      ; mult1*mult2 -> d0:d1

; now shift it over by 14 bits, effectively dividing by (b-a), the
; interpolation table spacing

        moveq   #14,d2                  ; d2 = shift size
        lsr.l   d2,d0

        moveq   #18,d2                  ; d2 = shift size
        lsl.l   d2,d1

        or.l    d1,d0                   ; return the result in d0

        move.l  (a7)+,d2                ; restore registers

#pragma END_ASM
#endif

   (void)mult1;
   (void)mult2;
}

